/**
 * @file mpu6050.c
 * @brief 陀螺仪
 *
 * @author 钟涛
 *
 * @date 2022-05-09
 *
 * @copyright 海南大学 - 追光者
 */
#ifdef APP_USING_MPU6050

#include <rtthread.h>
#include <rtdevice.h>
#include "drv_common.h"
#include "app_main.h"
#include "sensor_inven_mpu6xxx.h"
#include "inv_mpu.h"
#include "math.h"
#include "mpu6050.h"

#define DBG_TAG "MPU6050"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>

#define MPU6050_ADDR                     0x68

static struct mpu6xxx_device *i2c_bus;

rt_uint8_t MPU_Write_Len(rt_uint8_t addr, rt_uint8_t reg, rt_uint8_t len, rt_uint8_t *databuf)
{
    rt_int8_t res = 0;
#ifdef RT_USING_I2C
    struct rt_i2c_msg msgs;
    rt_uint8_t buf[50] = { 0 };
#endif
    buf[0] = reg;

    for (int i = 0; i < len; i++)
    {
        buf[i + 1] = databuf[i];
    }

    if (i2c_bus->bus->type == RT_Device_Class_I2CBUS)
    {
        msgs.addr = i2c_bus->i2c_addr; /* slave address */
        msgs.flags = RT_I2C_WR; /* write flag */
        msgs.buf = buf; /* Send data pointer */
        msgs.len = len + 1;

        if (rt_i2c_transfer((struct rt_i2c_bus_device *) i2c_bus->bus, &msgs, 1) == 1)
        {
            res = RT_EOK;
        }
        else
        {
            res = -RT_ERROR;
        }
    }
    return res;
}

rt_uint8_t MPU_Read_Len(rt_uint8_t addr, rt_uint8_t reg, rt_uint8_t len, rt_uint8_t *buf)
{
    rt_int8_t res = 0;
#ifdef RT_USING_I2C
    struct rt_i2c_msg msgs[2];
#endif
#ifdef RT_USING_SPI
    rt_uint8_t tmp;
#endif
    if (i2c_bus->bus->type == RT_Device_Class_I2CBUS)
    {
        msgs[0].addr = i2c_bus->i2c_addr; /* Slave address */
        msgs[0].flags = RT_I2C_WR; /* Write flag */
        msgs[0].buf = &reg; /* Slave register address */
        msgs[0].len = 1; /* Number of bytes sent */

        msgs[1].addr = i2c_bus->i2c_addr; /* Slave address */
        msgs[1].flags = RT_I2C_RD; /* Read flag */
        msgs[1].buf = buf; /* Read data pointer */
        msgs[1].len = len; /* Number of bytes read */

        if (rt_i2c_transfer((struct rt_i2c_bus_device *) i2c_bus->bus, msgs, 2) == 2)
        {
            res = RT_EOK;
        }
        else
        {
            res = -RT_ERROR;
        }
    }

    return res;
}

rt_uint8_t MPU_Write_Byte(rt_uint8_t reg, rt_uint8_t data)
{
    MPU_Write_Len(MPU6050_ADDR, reg, 1, &data);
    return 0;
}

rt_uint8_t MPU_Read_Byte(rt_uint8_t reg)
{
    uint8_t res;
    MPU_Read_Len(MPU6050_ADDR, reg, 1, &res);

    return res;
}

uint8_t MPU_Init(void)
{
    uint8_t res;
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80); //复位MPU6050
    delay_ms(100);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00); //唤醒MPU6050
    mpu6xxx_set_param(i2c_bus, MPU6XXX_ACCEL_RANGE, MPU6XXX_GYRO_RANGE_2000DPS);  //陀螺仪范围配置
    mpu6xxx_set_param(i2c_bus, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);     //加速度计
    mpu6xxx_set_param(i2c_bus, MPU6XXX_SAMPLE_RATE, 50);                       //采样频率

    MPU_Write_Byte(MPU_INT_EN_REG, 0X00);    //关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00); //I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00);   //关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80); //INT引脚低电平有效
    res = MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if (res == MPU_ADDR) //器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01); //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00); //加速度与陀螺仪都工作
        mpu6xxx_set_param(i2c_bus, MPU6XXX_SAMPLE_RATE, 50);                       //采样频率
    }
    else
    {
        return 1;
    }

    return 0;
}

int mpu6050_read(float *pitch, float* roll, float *yaw)
{
    int times = 5;

    if(rt_i2c_bus_lock((struct rt_i2c_bus_device *)i2c_bus->bus, 1000))
        return -RT_ETIMEOUT;

    while (times > 0)
    {
        if (mpu_dmp_get_data(pitch, roll, yaw) == 0)
        {
            LOG_D("pitch=%d, roll=%d, yaw=%d", (int )pitch, (int )roll, (int )yaw);
            break;
        }
        else
        {
            LOG_D("mpu_dmp_get_data fail");
        }
        if (--times == 0)
        {
            return -1;
        }
        rt_thread_mdelay(10);
    }

    rt_i2c_bus_unlock((struct rt_i2c_bus_device *)i2c_bus->bus);

    return 0;
}

void test_mpu6050(void)
{
    float pitch, roll, yaw;
    if (mpu6050_read(&pitch, &roll, &yaw) == 0)
    {
        LOG_I("test mpu6050 pass, pitch=%d, roll=%d, yaw=%d", (int )pitch, (int )roll, (int )yaw);
    }
    else
    {
        LOG_E("test mpu6050, fail.");
    }
}
MSH_CMD_EXPORT(test_mpu6050, test mpu6050);

extern float pitch, roll, yaw;
void mpu6050_thread(void *params)
{
    LOG_I("mpu6050 init ...");
    MPU_Init();
    while (mpu_dmp_init() != 0)
    {
        LOG_I("mpu_dmp_init...");
        rt_thread_mdelay(500);
    }
    rt_event_send(s_event, EVENT_MPU6050_INIT);
    LOG_I("mpu6050 init finshed.");

    while(1)
    {
        mpu6050_read(&pitch, &roll, &yaw);
        rt_thread_mdelay(20);
    }
}

int mpu6050_init(void)
{
    i2c_bus = (struct mpu6xxx_device *) mpu6xxx_init(MPU6050_I2C_BUS_NAME, MPU6050_ADDR);   //初始化MPU6050，测量单位为角速度，加速度

    if (!i2c_bus)
    {
        LOG_E("Can not find mpu6050!");
        return -1;
    }
    rt_thread_t thread= rt_thread_create("mpu6050", mpu6050_thread, NULL, 2048, PRIO_MPU6050, 10);

    // 启动陀螺仪线程
    //rt_thread_startup(thread);


    return 0;
}
INIT_DEVICE_EXPORT(mpu6050_init);

#endif /* APP_USING_MPU6050 */
